import math
from rclpy.node import Node
from sls_ugvInterface import UGVinterface


class UGVResource:
    # sigular instance
    _instance = None

    def __new__(cls, *args, **kwargs):
        if not cls._instance:
            cls._instance = super().__new__(cls)
        return cls._instance

    def __init__(self, main_node: Node, idStr: str):
        self.platform_interface = UGVinterface(main_node)
        self._idStr = idStr
        self.init_peripheral()

    # all peripheral init here
    def init_peripheral(self) -> None:
        pass

    # idStr
    def get_idStr(self) -> str:
        return self._idStr

    # get vehicle state
    def get_vehicle_speed_from_gps(self) -> float:
        # [lon, lat, alt, vel, vel_heading, yaw]
        lla_data = self.platform_interface.lla_data
        if lla_data is None:
            return 0.0  # TODO: handle None data

        return lla_data[3]  # vel

    def get_vehicle_speed_from_motor(self) -> float:
        K_wheel_radius = 0.21  # wheel radius [m]

        motor_data = self.platform_interface.motor_data
        if motor_data is None:
            return 0.0
        # [spd (rpm), trq (Nm), spd, trq]
        return (
            0.5 * (motor_data[0] - motor_data[2]) * 2 * math.pi / 60.0 * K_wheel_radius
        )  # speed (m/s)

    def get_steering_angle(self) -> float:
        """
        get steering angle from vehicle
        unit: [rad]
        """
        steer_data = self.platform_interface.steering_data
        if steer_data is None:
            return 0.0
        return self.platform_interface.chassis_sub.steering_angle[1]

    def get_gps_position(self) -> tuple[float, float, float]:
        # [lon, lat, alt]
        lla_data = self.platform_interface.lla_data
        if lla_data is None:
            return 0.0, 0.0, 0.0  # TODO: handle None data
        return (lla_data[0], lla_data[1], lla_data[2])
    
    def get_gps_yaw(self) -> float:
        # yaw
        lla_data = self.platform_interface.lla_data
        if lla_data is None:
            return 0
        return lla_data[5]

    def get_rear_motor_speed(self) -> tuple[float, float]:
        motor_data = self.platform_interface.motor_data
        if motor_data is None:
            return 0.0, 0.0
        return (motor_data[0], -motor_data[2])  # TODO: check the sign of speed

    def get_rear_motor_torque(self) -> tuple[float, float]:
        motor_data = self.platform_interface.motor_data
        if motor_data is None:
            return 0.0, 0.0
        return (motor_data[1], -motor_data[3])

    # set vehicle control command
    def pub_vehicle_cmd(
        self, *, mode: int, shift: int, speed: float, steer_angle: float, brake: int
    ) -> None:
        """
        param speed: speed of vehicle [m/s]
        param angle: steering angle of vehicle [rad]
        param shift: shift of vehicle [1:D, 2:N, 3:R]
        param brake: brake of vehicle [0:off, 1:on]
        enum ShiftLevel : uint8_t { D = 1, N, R };
        enum DriveMode : uint8_t { None = 0, Auto, PAD, Remote };

        """
        self.platform_interface.publish_chassis(
            mode=mode,
            shift=shift,
            speed=speed,
            steer_angle=steer_angle,
            brk=brake,
        )

        return
